
/*
 * ADC.c
 *
 *  Created on: 2021年6月1日
 *      Author: HP
 */
#include "ADC.h"

//是否传电感值到上位机0不发送，1短前瞻，2长前瞻
//flag
static uint8 collect_data_finish=0;
uint8 cross_pid_change_flag =0;
static uint8 round_num=0;
//写进菜单的flag
uint8 send_data_flag=0;

////ADC0可选引脚
//ADC0_CH0_A0   = 0*16 + 0,
//ADC0_CH1_A1,
//ADC0_CH2_A2,
//ADC0_CH3_A3,
//ADC0_CH4_A4,
//ADC0_CH5_A5,
//ADC0_CH6_A6,
//ADC0_CH7_A7,
//
////ADC1可选引脚
//ADC1_CH0_A8   = 1*16 + 0,
//ADC1_CH2_A10  = 1*16 + 2,
//ADC1_CH3_A11,
//ADC1_CH4_A12,
//ADC1_CH5_A13,
//
////ADC2可选引脚
//ADC2_CH0_A16  = 2*16 + 0,
//ADC2_CH1_A17  = 2*16 + 1,
//ADC2_CH4_A20  = 2*16 + 4,
//ADC2_CH5_A21  = 2*16 + 5,
//
////ADC3可选引脚
//ADC3_CH0_A24  = 3*16 + 0,
//ADC3_CH1_A25  = 3*16 + 1,
//
////ADC8可选引脚
//ADC8_CH3_A35  = 8*16 + 3,
//ADC8_CH4_A36,
//ADC8_CH5_A37,
//ADC8_CH6_A38,
//ADC8_CH7_A39,
//ADC8_CH10_A42 = 8*16 + 10,
//ADC8_CH11_A43,
//ADC8_CH12_A44,
//ADC8_CH13_A45,
//ADC8_CH14_A46,
//ADC8_CH15_A47,
//长前瞻电感所用到的通道,使AD通道 0到6分别对应 左1横、右一横、左竖、右竖、左二横、右二横、中间电感
uint8 channel_name[AD_NUM]={ADC8_CH3_A35,ADC1_CH4_A12,ADC8_CH4_A36,ADC1_CH5_A13,ADC3_CH0_A24,ADC2_CH0_A16,ADC2_CH4_A20};
uint8 channel_adc[AD_NUM]={ADC_8,ADC_1,ADC_8,ADC_1,ADC_3,ADC_2,ADC_2};
//AI电感采数数组
uint8 channel_AI[AI_NUM]={ADC1_CH3_A11,ADC0_CH1_A1,ADC1_CH2_A10,ADC0_CH2_A2,ADC1_CH0_A8,ADC0_CH3_A3,ADC0_CH5_A5,ADC8_CH7_A39,ADC8_CH5_A37,ADC8_CH10_A42,ADC8_CH6_A38,ADC0_CH7_A7,ADC0_CH6_A6,ADC0_CH4_A4};
uint8 channel_AI_adc[AI_NUM]={ADC_1,ADC_0,ADC_1,ADC_0,ADC_1,ADC_0,ADC_0,ADC_8,ADC_8,ADC_8,ADC_8,ADC_0,ADC_0,ADC_0};
//用来存放电感采集完后的平均值
static float Average[AD_NUM]={0};
static float Average_AI[AI_NUM]={0};
//全局变量
float AD_Short[AI_NUM]={0};
float AD_Long[AD_NUM]={0};
float Max_Short[AI_NUM]={0};
float Max_Long[AD_NUM]={0};
Judgment_threshold long_theshold,short_theshold,AI_theshold;
Judgment_threshold_new long_theshold_new;
uint8 type_of_road = 0;
uint8 low_round_flag=0;
int16 up_speed_ad=0;
Encoder cancel_right;
uint8 right_round=0;
uint8 into_the_ring=0;
uint8 left_round=0;
uint8 out_the_ring=0;
uint8 in_the_round=0;
uint8 straight_curve_flag=0;
uint8 send_test[17];

int32 num_of_90_curve=0;
//识别道路类型的判别量
float devithion_divided_num=0;

void PIDLS_process(void)
{
    AD_Sample();
    AD_uniformization();
    PID_recognize_road();
    PID_get_err();
    Send_train_data();
#if defined  (SEND_AD_NUM_BUTTON) && (SEND_AD_NUM_BUTTON != 0)
    Send_short_data();
#endif
}

void Collect_data(void)
{
    Short_AD_Sample();

    if(collect_max_flag!=1)
    {
        if(collect_data_finish==0)
        {
            Short_AD_normalization();
            collect_data_finish=1;
        }

    }else
    {
        for(uint8 i=0;i<AI_NUM;i++)
        {
            if(Average_AI[i] > Max_Short[i])
            {
              Max_Short[i] = Average_AI[i];
            }
            AD_Short[i] = Average_AI[i];
        }
    }
    Send_train_data();
#if defined  (SEND_AD_NUM_BUTTON) && (SEND_AD_NUM_BUTTON != 0)
    Send_short_data();
#endif
}
void Long_Control(void)
{

        Long_AD_Sample();
        if(collect_max_flag!=1)
        {
            Long_AD_normalization();
        }else
        {
            for(uint8 i=0;i<AD_NUM;i++)
            {
                if(Average[i] > Max_Long[i])
                {
                    Max_Long[i] = Average[i];
                }
                AD_Long[i] = Average[i];
            }
        }

        PID_recognize_road();
        PID_get_err();
}

void NNOM_process(void)
{
    Short_AD_Sample();
    if(collect_max_flag!=1)
    {
        Short_AD_normalization();
    }else
    {
        for(uint8 i=0;i<AI_NUM;i++)
        {
            if(Average_AI[i] > Max_Short[i])
            {
              Max_Short[i] = Average_AI[i];
            }
            AD_Short[i] = Average_AI[i];
        }
    }

}

void AD_Sample(void)
{
    if(short_process_flag==0)
    {
        Long_AD_Sample();
    }
    Short_AD_Sample();
}

void AD_uniformization(void)
{
    if (collect_max_flag!=1)
    {
        if(collect_data_finish==0)
        {
            if(short_process_flag==0)
            {
                Long_AD_normalization();
            }
            Short_AD_normalization();
            //AD[i+AD_NUM] = transfer_AI[i];
            collect_data_finish=1;
        }
    }
    else if (collect_max_flag==1)
    {
        if (short_process_flag==0)
        {
            for(uint8 i=0;i<AD_NUM;i++)
            {
                if(Average[i] > Max_Long[i])
                {
                  Max_Long[i] = Average[i];
                }
                AD_Long[i] = Average[i];
            }
        }
        for(uint8 i=0;i<AI_NUM;i++)
        {
            if(Average_AI[i] > Max_Short[i])
            {
              Max_Short[i] = Average_AI[i];
            }
            AD_Short[i] = Average_AI[i];
        }

    }
}

void PID_recognize_road(void)
{
//    if (short_process_flag==0)
//    {
        Long_recognize_road();
//    }
//    else if(short_process_flag==1)
//    {
////        AI_recognize_road();
//    }
}

void PID_get_err(void)
{
//    if(short_process_flag!=1)
//     {
         if(type_of_road==0)

         {
             //横电感差比积，右一减左一
             Long_err.error_now=(AD_Long[1]-AD_Long[0])/(AD_Long[1]*AD_Long[0]+1);
             //竖横电感差比积，右减左+10减小竖电感差比积抖动
//             err_ad_now[1]=(AD_Long[3]-AD_Long[2])/(AD_Long[3]*AD_Long[2]+10);
             //给竖电感较小的权重
//             err_synthetical_now=0*err_ad_now[1]+1*err_ad_now[0];
         }
         else if(type_of_road==10||type_of_road==20)
         {
             Long_err.error_now = -2.0f;
         }else if(type_of_road==11||type_of_road==21)
         {
             Long_err.error_now = 2.0f;
         }
//     }else
//     {
//         Short_err.error_now=(AD[8]-AD_Short[0])/(AD[8]*AD_Short[0]+1);
//     }

}

void Send_short_data(void)
{
    if(send_data_flag==0)
    {
        float sen_ad_1[20]={0};
        for(uint8 i=0;i<11;i++)
         {
             sen_ad_1[i]=AD_Short[i];
         }
        sen_ad_1[11]=AD_Short[0]+AD_Short[1];
        sen_ad_1[12]=AD_Short[0]+AD_Short[1]+AD_Short[6];
        sen_ad_1[13]=200*fabs(AD_Short[2]-AD_Short[3])/(AD_Short[0]+AD_Short[1]+AD_Short[6]);
        sen_ad_1[14]=200*fabs(AD_Short[2]-AD_Short[3])/(AD_Short[0]+AD_Short[1]);
        if(straight_curve_flag==0)//直角特征&&straight_curve_flag=0
        {
//            AI_angle.acc_of_90_curve_angle=0;
            if(sen_ad_1[14]>AI_theshold.max_remember_90_)
            {
                straight_curve_flag=1;
            }
        }
        else
        {
            if(sen_ad_1[14]<AI_theshold.min_remember_90_)//取消直角特征
            {
                straight_curve_flag=0;
                num_of_90_curve++;
                curve_distance.encoder_num=0;
            }
        }
        SmartCar_VarUpload(sen_ad_1,16);
    }
}


void Send_train_data(void)
{
    if(send_data_flag)
    {
        if(collect_data_finish)
        {
            //14个AI电感
            //1个舵机
            //1个电机
            //1个标志位0x5a
            //17个数据
            for(uint8 i=0;i<AI_NUM;i++)
            {
                send_test[i]=(uint8)((int8)((int32)AD_Short[i]-128));
//                send_test[i]=(uint8)((int32)AD_Short[i]-128);
            }
            //电机和舵机
            send_test[14]=(uint8)((int8)((int32)(127*((pwm_servo-servo_mid)/1.8f))));
            //偏差
            send_test[15]=(uint8)((int8)(int32)(127*(2/(1+exp(-4*Long_err.error_now))-1.0f)));
            send_test[16]=0x5a;
            SmartCar_Uart_Transfer(send_test,17,2);
            collect_data_finish=0;
        }
    }else if(send_data_flag!=1)
    {
        if(collect_data_finish)
        {
            collect_data_finish=0;
        }
    }
}

void Long_AD_Sample(void)
{
    uint16 LAD_Sample[AD_NUM][SampleTimes];
    float transfer[AD_NUM]={0};
    for (uint8 h=0;h<AD_NUM;h++)
        {
            for(uint8 i=0;i<SampleTimes;i++)
            {
             /*获取采样初值*/
#if     defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==12U)
                LAD_Sample[h][i] = adc_convert(channel_adc[h], channel_name[h], ADC_12BIT);
#elif   defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==10U)
                LAD_Sample[h][i] = adc_convert(channel_adc[h], channel_name[h], ADC_10BIT);
#elif   defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==8U)
                LAD_Sample[h][i] = adc_convert(channel_adc[h], channel_name[h], ADC_8BIT);
#endif
            }
        }
//看效果决定加不加回来
//    for(uint8 i=0;i<AD_NUM;i++)
//    {
//        for(uint8 j=0;j<=SampleTimes-1;j++)
//        {
//             if(LAD_Sample[i][j]>BIT_MAX)//剔除毛刺信号
//             {
//                 LAD_Sample[i][j]=BIT_MAX;
//             }
//        }
//    }
    for(uint8 k=0;k<AD_NUM;k++)
    {
        for(uint8 i=0;i<SampleTimes;i++)
        {
            transfer[k]+=(float)((int16)LAD_Sample[k][i]);
        }
        Average[k]=transfer[k]/SampleTimes;
        if( Average[k] < MinADGet_long )
        {
            Average[k] = MinADGet_long;
        }
    }
}
void Short_AD_Sample(void)
{

    uint16 SAD_Sample[AI_NUM][SampleTimes_AI];
    float transfer_AI[AI_NUM]={0};
    for (uint8 h=0;h<AI_NUM;h++)
    {
        for(uint8 i=0;i<SampleTimes_AI;i++)
        {
#if     defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==12U)
            SAD_Sample[h][i] = adc_convert(channel_AI_adc[h], channel_AI[h], ADC_12BIT);
#elif   defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==10U)
            SAD_Sample[h][i] = adc_convert(channel_AI_adc[h], channel_AI[h], ADC_10BIT);
#elif   defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==8U)
            SAD_Sample[h][i] = adc_convert(channel_AI_adc[h], channel_AI[h], ADC_8BIT);
#endif
        }
    }

    for(uint8 i=0;i<AI_NUM;i++)
           {
               for(uint8 j=0;j<SampleTimes_AI;j++)
               {
                    if(SAD_Sample[i][j]>BIT_MAX)//剔除毛刺信号
                    {
                        SAD_Sample[i][j]=BIT_MAX;
                    }
               }
           }
    for(uint8 k=0;k<AI_NUM;k++)
          {
               for(uint8 i=0;i<SampleTimes_AI;i++)
               {
                   transfer_AI[k]+=(float)((int16)SAD_Sample[k][i]);
               }
               Average_AI[k]=transfer_AI[k]/SampleTimes_AI;
               if( Average_AI[k] < MinADGet_AI)
               {
                  Average_AI[k] = MinADGet_AI;
               }
           }

}


void Long_AD_normalization(void)
{
    AD_Long[0] = (135*Average[0])/Max_Long[0];
    AD_Long[1] = (135*Average[1])/Max_Long[1];
    AD_Long[2] = (140*Average[2])/Max_Long[2];
    AD_Long[3] = (140*Average[3])/Max_Long[3];
//    AD_Long[4] = (125*Average[4])/Max_Long[4];
//    AD_Long[5] = (125*Average[5])/Max_Long[5];
    AD_Long[6] = (127*Average[6])/Max_Long[6];
    for(uint8 i= 0;i<AD_NUM;i++)
    {
        if(AD_Long[i]>BIT_MAX)
        AD_Long[i]=BIT_MAX;
    }
}
void Short_AD_normalization(void)
{
    AD_Short[0] = (140*Average_AI[0])/Max_Short[0];//(K = 140期待解决直线走不正的问题)
    AD_Short[1] = (140*Average_AI[1])/Max_Short[1];
    AD_Short[2] = (210*Average_AI[2])/Max_Short[2];
    AD_Short[3] = (210*Average_AI[3])/Max_Short[3];
    AD_Short[4] = (140*Average_AI[4])/Max_Short[4];
    AD_Short[5] = (140*Average_AI[5])/Max_Short[5];
    AD_Short[6] = (127*Average_AI[6])/Max_Short[6];
    AD_Short[7] = (140*Average_AI[7])/Max_Short[7];
    AD_Short[8] = (140*Average_AI[8])/Max_Short[8];
    AD_Short[9] = (127*Average_AI[9])/Max_Short[9];
    AD_Short[10] = (127*Average_AI[10])/Max_Short[10];
    AD_Short[11] = (127*Average_AI[11])/Max_Short[11];
    AD_Short[12] = (127*Average_AI[12])/Max_Short[12];
    AD_Short[13] = (127*Average_AI[13])/Max_Short[13];
    for(uint8 i=0;i<AI_NUM;i++)
    {
      if(AD_Short[i]>BIT_MAX)
       {
                AD_Short[i]=BIT_MAX;
       }
    }
}

void Long_recognize_road(void)
{
    //直角弯标志位
    float L_vertical,R_vertical,L_horizontal,R_horizontal,Mid_ad,L_45Curve,R_45Curve;
    L_horizontal=AD_Long[0];
    R_horizontal=AD_Long[1];
    L_vertical=AD_Long[2];
    R_vertical=AD_Long[3];
//    //用于区分45度角和直角
//    L_45Curve=AD_Long[4];
//    R_45Curve=AD_Long[5];
    //中间横电感
    Mid_ad=AD_Long[6];
    float num_of_two=L_horizontal+R_horizontal;
    float shu_cha=L_vertical-R_vertical;


//直角v2.0
     float num_of_vehecal=L_vertical+R_vertical;
     devithion_divided_num=200*shu_cha/num_of_two;
     float f_devithion_divided_num=fabs(devithion_divided_num);
     if(type_of_road!=20&&type_of_road!=21)
     {
         if(f_devithion_divided_num<=long_theshold_new.turn_curve_small||num_of_two>long_theshold_new.round_num||num_of_vehecal>long_theshold_new.cross_verhecal_num)//||L_vertical+R_vertical>long_theshold_new.cross_verhecal_num
         {
             type_of_road=0;//直线
         }else
         {
              if(f_devithion_divided_num>long_theshold_new.turn_curve_small&&f_devithion_divided_num<long_theshold_new.turn_curve_large)//
              {
                      if(devithion_divided_num>0)
                      {
                          Long_servo_angle.angle_small_left=Get_Small_angle(f_devithion_divided_num,long_theshold_new.left_angle_small);
                          type_of_road=10;//左拐小
                      }

                      else
                      {
                          Long_servo_angle.angle_small_right=Get_Small_angle(f_devithion_divided_num,long_theshold_new.right_angle_small);
                          type_of_road=11;//右拐小
                      }

              }
              else if(f_devithion_divided_num>long_theshold_new.turn_curve_large)
              {
                  if(devithion_divided_num>0)
                  type_of_road=20;//左拐大
                  else
                  type_of_road=21;//右拐大
              }
         }
     }else if(type_of_road==20||type_of_road==21)
     {
        if(num_of_two>long_theshold_new.cancel_90_status&&fabsf(shu_cha)<long_theshold_new.cacel_90_shu)
        {
            type_of_road=0;
        }
     }

//     if(type_of_road==0)
//     {
//        if(R_vertical>long_theshold.cross_pid_ad&&L_vertical>long_theshold.cross_pid_ad&&Mid_ad>105.0f)
//        {
//            cross_pid_change_flag=1;
//        }else
//        {
//            cross_pid_change_flag=0;
//        }
//     }
//直角v1.0
//    if (R_vertical-L_vertical>long_theshold.turn_90_curve&&L_vertical<long_theshold.is_cross_not_90&&L_horizontal+R_horizontal<long_theshold.is_45_not_90)
//    {
//        type_of_road=11;
//
//    }
//    if (L_vertical-R_vertical>long_theshold.turn_90_curve&&R_vertical<long_theshold.is_cross_not_90&&L_horizontal+R_horizontal<long_theshold.is_45_not_90)
//    {
//        type_of_road=10;
//    }
//
//
//    if(type_of_road==0)
//    {
//       if(R_vertical>long_theshold.cross_pid_ad&&L_vertical>long_theshold.cross_pid_ad&&Mid_ad>105.0f)
//       {
//           cross_pid_change_flag=1;
//       }else
//       {
//           cross_pid_change_flag=0;
//       }
//    }
//    else if (type_of_road==11||type_of_road==10)
//    {
////        if (L_horizontal+R_horizontal>long_theshold.cancel_90_status&&fabsf(L_vertical-R_vertical)<long_theshold.cacel_90_shu)
//        if (L_horizontal+R_horizontal>long_theshold.cancel_90_status&&fabsf(L_vertical-R_vertical)<long_theshold.cacel_90_shu)//统一变量，为了测试
//        {
//            type_of_road=0;
//        }
//    }
}

float Get_Small_angle(float var,float max_angle)
{
    float var_min=long_theshold_new.turn_curve_small;
    float var_max=long_theshold_new.turn_curve_large;
    float result=max_angle*(var-var_min)/(var_max-var_min);
    return result;

}
void AI_recognize_road(void)
{
    float SL_horizontal,SR_horizontal,SMid_ad,SL_vertical_2,SR_vertical_2,SL_vertical_1,SR_vertical_1;

    SMid_ad=AD_Short[6];
    SL_horizontal=AD_Short[0];
    SR_horizontal=AD_Short[1];
    SL_vertical_1=AD_Short[2];
    SR_vertical_1=AD_Short[3];
    //判断圆环方向的电感
    SL_vertical_2=AD_Short[10];
    SR_vertical_2=AD_Short[9];
    float num_of_three_horizen= SL_horizontal+SR_horizontal+SMid_ad;
    float fabs_devithion_divided_num_short=200*fabsf(AD_Short[2]-AD_Short[3])/(AD_Short[0]+AD_Short[1]);
//    type_of_road=0;
////    if(num_of_three_horizen>up_speed_ad)
////    {
////        up_speed_flag=1;
////    }else
////    {
////        up_speed_flag=0;
////    }

    if(num_of_three_horizen>AI_theshold.round_mid&&Recognize_Round.encoder_num>240000)
    {
        in_the_round=1;
        Recognize_Round.encoder_num=0;
        type_of_road=50;
        if(right_round==0&&left_round==0)
        {
            round_num++;
            AI_angle.acc_of_complet_round_angle=0.0f;
                //竖电感判断环岛
                if(SL_vertical_2>SR_vertical_2)
                {
                    left_round=1;
                    right_round=0;
                }
                else if(SL_vertical_2<SR_vertical_2)
                {
                    right_round=1;
                    left_round=0;
                }
        }
//            强制进环岛，在菜单里可以设置
//                switch(round_num)
//                {
//                    case 1U:
//                    {
//                        left_round=0;
//                        right_round=0;
//                        into_the_ring=1;
//                    }
//                    break;
//                    case 2U:
//                    {
//                        left_round=0;
//                        right_round=0;
//                        into_the_ring=1;
//                    }
//                    break;
//                    case 3U:
//                    {
//                        left_round=0;
//                        right_round=0;
//                        into_the_ring=1;
//                    }
//                    break;
//                    case 4U:
//                    {
//                        left_round=0;
//                        right_round=0;
//                        into_the_ring=1;
//                    }
//                    break;
//                    case 5U:
//                    {
//                        left_round=0;
//                        right_round=0;
//                        into_the_ring=1;
//                    }
//                    break;
//                }

    }
    if(type_of_road==50)
    {
        if(fabsf(AI_angle.acc_of_complet_round_angle)>=AI_theshold.angle||num_of_three_horizen<AI_theshold.cancel_into_round)
        {
            //短前瞻跑圆环
#if defined  (AI_YUANHUAN_BUTTON) && (AI_YUANHUAN_BUTTON == 0)
            type_of_road=51;
#endif
#if defined  (AI_YUANHUAN_BUTTON) && (AI_YUANHUAN_BUTTON == 1)
//AI跑圆环
            type_of_road=0;
#endif

        }
    }
//AI跑圆环
#if defined  (AI_YUANHUAN_BUTTON) && (AI_YUANHUAN_BUTTON == 0)
    else if(type_of_road==51)
    {
        AI_round_err.error_now=(SR_horizontal-SL_horizontal)/(SL_horizontal*SR_horizontal+1);

        if(AI_angle.acc_of_complet_round_angle<AI_theshold.low_speed_angle)
        {
            low_round_flag=1;
        }
        else if(AI_angle.acc_of_complet_round_angle>AI_theshold.angle_out)
        {
            in_the_round=0;
            low_round_flag=0;
            type_of_road=0;
            right_round=0;
            left_round=0;
        }
    }
#endif
    else if(type_of_road==0)

//    if(type_of_road==0)
    {
        if(straight_curve_flag==0)//直角特征&&straight_curve_flag=0
        {
//            AI_angle.acc_of_90_curve_angle=0;
            if(fabs_devithion_divided_num_short>AI_theshold.max_remember_90_)
            {
                straight_curve_flag=1;
            }
        }
        else
        {
            if(fabs_devithion_divided_num_short<AI_theshold.min_remember_90_)//取消直角特征
            {
                straight_curve_flag=0;
                num_of_90_curve++;

            }
        }
    }

}

float Get_distance(VADCN_enum channel_module,VADC_CHN_enum channel_num,VADC_RES_enum read_res)
{
    int16 get_distancce_adc[READ_DISTANCE_TIME]={0};
    int32 get_distancce_num=0;
    float get_distancce_averange=0;
    float Right_x,distance;
    for(uint8 i=0;i<READ_DISTANCE_TIME;i++)
    {
        get_distancce_adc[i]=adc_convert(channel_module, channel_num, read_res);
        get_distancce_num+=get_distancce_adc[i];
    }
    get_distancce_averange=(float)get_distancce_num/READ_DISTANCE_TIME;
    Right_x=get_distancce_averange*3.30/4096;
    distance=26.481*pow(Right_x,(-1.05));
    if(distance>80.00) distance=80.00;         //限幅
    if(distance<8.00) distance=8.00;
    return distance;
}
